
clc;clear;
format longg
%% Constants
GM_sun=1.327124400*10^(11);     %Sun
GM_earth=3.986004418*10^(5);    %Earth
GM_moon = 4.9048695*10^(3);     %Moon
%[km^3/s^2] Gravitational Parameters

u_se = GM_earth / (GM_earth+GM_sun); % Sun Earth CRTBP constant
u_em = GM_moon / (GM_moon+GM_earth); % Earth Moon CRTBP constant
%[] Mu constant

r12_em = 384400;            % Distance between the Earth and the Moon
TU_em = 3.75676967e5;       % Time unit
LU_em = r12_em;             % Distance unit
VU_em = LU_em/TU_em;        % Velocity unit

r12_se = 1.4959965e+8;      % Distance between sun and the earth
TU_se = 5.0226757e+6;       % Time unit 
LU_se = r12_se;             % Length unit
VU_se = LU_se/TU_se;        % Velocity unit

epsilon=2*10^(-5);
%[] Parameter for altering the halo orbit's state

a = load('EML2_Northern.mat');                  % Load halo orbit initial conditions
UTC_Moon_Arrival = [2022 6 30 12 00 00];        % Arrival Time
JD_Moon_Arrival = JulianDate(UTC_Moon_Arrival); % Arrival time
%% Load Earth and Moon data
load('EarthDataCircularized.mat');      % Load circularized earth data
load('MoonDataCircularized.mat');       % Load circularized moon data
 
JD_EM = Julian_Date_EM;     % ALLOCATE
JD_SE = Julian_Date_SE;     % ALLOCATE
Res = Rearths;              % ALLOCATE
Ves = Vearths;              % ALLOCATE
Rme = Rmoone;               % ALLOCATE
Vme = Vmoone;               % ALLOCATE

%% Load manifolds.
load('Candidate_stable_EM_atSE.mat')     % Load stable manifold from Earth-Moon CRTBP
load('Candidates_unstable_SE.mat');      % Load unstable manifold from Sun-Earth CRTBP
n = length(t_unstable_inside);

%% Define poincare section
p1 = [1-u_se;0;0];      % Point at the position of Earth
p2 = [2;0;0];           % Position in x axis
p3 = [1-u_se;0;-1];     % Point -1 below the position of Earth.
%[] Points in space before rotation

angle =0;              % Rotation axis positive counter-clock wise from +x direction starting from Earth
%[] Angle of rotation.

T = Rotation([0;0;1],angle,'Degrees');  % Rotation matrix about z axis
p2_new = T * p2;                        % New p2 point.
%[] Calculate new p2 point

Vec1 = p2_new - p1;         % Vector from Earth to rotating point
Vec2 = p3-p1;               % Vector from p1 to p3
nhat = cross(Vec1,Vec2);    % Perpendicular vector
%[] Define normal vector.

plane_eq = @(x,y,z) nhat(1)*(x-p1(1))+nhat(2)*(y)+nhat(3)*(z);  % Plane equation 
%[] Define the poincare section.

%% Find the points in poincare section.
% Each nested for loop is finding the index where the sign change, and
% saving the position in cell matrix.
% 
% All points are saved in x-z plane for ease of plotting 

% R_sat/e = R_sat/cm - R_e/cm
for ii = 1:length(S_manifold_crtbp_se)
    %[] For all cell of trajectories
    
    pointdata = plane_eq(S_manifold_crtbp_se{ii}(1,:),S_manifold_crtbp_se{ii}(2,:),S_manifold_crtbp_se{ii}(3,:));
    %[] Put the positions in the plane equation
    
    ind_EM_stable{ii} = find(diff(sign(pointdata)));
    %[] Get the points where the sign change: Point of interestion's index
    
    for jj = 1:length(ind_EM_stable{ii})
        %[] for number of crossing points,
        a = S_manifold_crtbp_se{ii}(1:3,ind_EM_stable{ii}(jj));
        %[] Get the position data in crtbp
        
        deg = atan2d(a(2),a(1));
        %[] Find the deg wrt cm
        
        T2 = Rotation([0;0;1],deg,'Degrees');
        %[] find rotation matrix
        
        poincare_EM_stable{ii}(:,jj) = transpose(T2) * S_manifold_crtbp_se{ii}(1:3,ind_EM_stable{ii}(jj));
        %[] Poincare points.
    end
end


% for ii = 1:length(S_stable_inside)
%     pointdata = plane_eq(S_stable_inside{ii}(1,:),S_stable_inside{ii}(2,:),S_stable_inside{ii}(3,:));
%     ind_SE_stable{ii} = find(diff(sign(pointdata)));
%     for jj = 1:length(ind_SE_stable{ii})
%         a = S_stable_inside{ii}(1:3,ind_SE_stable{ii}(jj));
%         deg = atan2d(a(2),a(1));
%         T2 = Rotation([0;0;1],deg,'Degrees');
%         poincare_SE_stable{ii}(:,jj) = transpose(T2) * S_stable_inside{ii}(1:3,ind_SE_stable{ii}(jj)) ;
%     end
% end


for ii = 1:length(S_unstable_inside)
    pointdata = plane_eq(S_unstable_inside{ii}(1,:),S_unstable_inside{ii}(2,:),S_unstable_inside{ii}(3,:));
    ind_SE_unstable{ii} = find(diff(sign(pointdata)));
    for jj = 1:length(ind_SE_unstable{ii})
        a = S_unstable_inside{ii}(1:3,ind_SE_unstable{ii}(jj));
        deg = atan2d(a(2),a(1));
        T2 = Rotation([0;0;1],deg,'Degrees');
        poincare_SE_unstable{ii}(:,jj) = transpose(T2) * S_unstable_inside{ii}(1:3,ind_SE_unstable{ii}(jj)) ;
    end
end


%% Plot poincare section

figure;
hold on;
%[] Open figure, and hold.

for kk = 1:length(poincare_EM_stable)
    %[] For all points in the poincare section
    
    n = size(poincare_EM_stable{kk});
    %[] Find number of points
    
    for gg = 1:n(2)
        %[] for number of points
        posorneg = plane_eq(S_manifold_crtbp_se{kk}(1,ind_EM_stable{kk}(gg)),...
                            S_manifold_crtbp_se{kk}(2,ind_EM_stable{kk}(gg)),...
                            S_manifold_crtbp_se{kk}(3,ind_EM_stable{kk}(gg)));      
        %[] Check which side the point is coming from positive or negative
        
        if posorneg > 0
           %[] If point is coming from positive
           
            plot(poincare_EM_stable{kk}(1,gg),poincare_EM_stable{kk}(3,gg),'k+','MarkerSize',10)    % " + " if coming from negative to positive
            %[] Plot positive points
            
        else
            plot(poincare_EM_stable{kk}(1,gg),poincare_EM_stable{kk}(3,gg),'k.','MarkerSize',10)    % " . " if coming from negative to positive
            %[] else point the negative points
        
        end
    end
end

% for kk = 1:length(poincare_SE_stable)
%     n = size(poincare_SE_stable{kk});
%     for gg = 1:n(2)
%         posorneg = plane_eq(S_stable_inside{kk}(1,ind_SE_stable{kk}(gg)),...
%                             S_stable_inside{kk}(2,ind_SE_stable{kk}(gg)),...
%                             S_stable_inside{kk}(3,ind_SE_stable{kk}(gg)));
%         if posorneg > 0
%             plot(poincare_SE_stable{kk}(1,gg),poincare_SE_stable{kk}(3,gg),'b+','MarkerSize',10)
%         else
%             plot(poincare_SE_stable{kk}(1,gg),poincare_SE_stable{kk}(3,gg),'b.','MarkerSize',10)
%         end
%     end
% end

for kk = 1:length(poincare_SE_unstable)
    n = size(poincare_SE_unstable{kk});
    for gg = 1:n(2)
        posorneg = plane_eq(S_unstable_inside{kk}(1,ind_SE_unstable{kk}(gg)),...
                            S_unstable_inside{kk}(2,ind_SE_unstable{kk}(gg)),...
                            S_unstable_inside{kk}(3,ind_SE_unstable{kk}(gg)));
        if posorneg > 0
            plot(poincare_SE_unstable{kk}(1,gg),poincare_SE_unstable{kk}(3,gg),'b+','MarkerSize',10)
        else
            plot(poincare_SE_unstable{kk}(1,gg),poincare_SE_unstable{kk}(3,gg),'b.','MarkerSize',10)
        end
    end
end
xlabel('\Sigma_x');
ylabel('\Sigma_y');
legend('From pos','From neg')

%% Find the Two closes points at poincare section.

% Let's make them just a normal state set vector
poincare_SE_unstable_vec = [];
poincare_EM_stable_vec = [];
%[] Make vector for vectorizing the cell data.

for ii = 1:length(poincare_SE_unstable)
    poincare_SE_unstable_vec = [poincare_SE_unstable_vec,poincare_SE_unstable{ii}];
end

for ii = 1:length(poincare_EM_stable)
   poincare_EM_stable_vec = [poincare_EM_stable_vec,poincare_EM_stable{ii}]; 
end
%[] Make a normal vector version

for ii = 1:length(poincare_SE_unstable_vec)
   
    relDist_3xn = poincare_EM_stable_vec-poincare_SE_unstable_vec(:,ii);
    %[] Get relative position vector
    
    for jj = 1:length(relDist_3xn)
        relDistMatrix(ii,jj) = norm(relDist_3xn(:,jj));
        %[] Get the relative position value
        
    end
    
end

[unstable_ind,stable_ind] = find(min(min(relDistMatrix))==relDistMatrix);
%[] Find minimum position difference's unstable index and stable index.

plot(poincare_SE_unstable_vec(1,unstable_ind),poincare_SE_unstable_vec(3,unstable_ind),'rd')
plot(poincare_EM_stable_vec(1,stable_ind),poincare_EM_stable_vec(3,stable_ind),'rd')
%[] Plot the closest points in poincare section

count_stable = 1;
count_unstable = 1;
%[] Counter

for ii = 1:length(poincare_SE_unstable)
    %[] For all cell
    n = size(poincare_SE_unstable{ii});
    for jj = 1:n(2)
        %[] for all points in a cell
        if poincare_SE_unstable{ii}(1,jj) == poincare_SE_unstable_vec(1,unstable_ind)
           %[] If the point is equal to the point of interest (unstabld_ind)
            S_unstable_inside_best = S_unstable_inside{ii};
            %[] Save the state vector.
        end
    end
end

for ii = 1:length(poincare_EM_stable)
    n = size(poincare_EM_stable{ii});
    for jj = 1:n(2)
        if poincare_EM_stable{ii}(1,jj) == poincare_EM_stable_vec(1,stable_ind)
            S_manifold_crtbp_se_best = S_manifold_crtbp_se{ii};
        end
    end
    
end


%% Plot Plane in space
figure;
hold on;
axis equal
%[] open figure
% 
% for ii = 1:length(S_unstable_inside)
%     plot3(S_unstable_inside{ii}(1,:),S_unstable_inside{ii}(2,:),S_unstable_inside{ii}(3,:),'b','LineWidth',0.5)
% end
% %[] Plot unstable manifold from Sun-Earth system
% 
% for ii = 1:length(S_manifold_crtbp_se)
%     plot3(S_manifold_crtbp_se{ii}(1,:),S_manifold_crtbp_se{ii}(2,:),S_manifold_crtbp_se{ii}(3,:),'k','LineWidth',0.5)
% end
% %[] Plot stable manifold from Earth-Moon system

plot3(S_unstable_inside_best(1,:),S_unstable_inside_best(2,:),S_unstable_inside_best(3,:),'r.')
plot3(S_manifold_crtbp_se_best(1,:),S_manifold_crtbp_se_best(2,:),S_manifold_crtbp_se_best(3,:),'r.')
%[] Plot the two closest point in poincare section's trajectories.




